/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PWM;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Watchdog;
import edu.wpi.first.wpilibj.camera.AxisCamera;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends IterativeRobot {

    protected Joystick left;
    protected Joystick right;

    protected RobotDrive driveTrain;

    protected Compressor com;

    protected Solenoid one;
    protected Solenoid two;

    protected Servo servo1;
    protected Servo servo2;

    protected AxisCamera camera;

    protected Vision vision;

    //TODO Tune and implement
    protected final int m_minMajor = 20;
    protected final int m_maxMajor = 100;
    protected final int m_minMinor = 20;
    protected final int m_maxMinor = 100;

    //CONSTANTS

    //Joysticks
    protected final int C_JOYSTICK_LEFT = 1;
    protected final int C_JOYSTICK_RIGHT = 2;

    //DriveTrain
    protected final int C_LEFT_DRIVETRAIN_MOTOR = 1;
    protected final int C_RIGHT_DRIVETRAIN_MOTOR = 2;

    protected PWM out;

    //INIT ///////////////////////////////////////////////////////////////////////

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        System.out.println("Init");

        out = new PWM(4);

        initJoysticks();
        initDriveTrain();
        initPneumatics();
        //initServos();
        initCamera();
    }

    /**
     * This method is used to initalize the standard two drive train joysticks
     * used for tank drive.
     */
    protected void initJoysticks(){
        left = new Joystick(C_JOYSTICK_LEFT);
        right = new Joystick(C_JOYSTICK_RIGHT);
    }

    /**
     * This method is used to initalize the standard two motors for
     * the basic tank drive.
     */
    protected void initDriveTrain(){
        driveTrain = new RobotDrive(C_LEFT_DRIVETRAIN_MOTOR,
                C_RIGHT_DRIVETRAIN_MOTOR);
    }

    /**
     * This method is used to initalize the compressor and the
     * two solenoids for the one piston.
     */
    protected void initPneumatics(){
        com = new Compressor(1, 1);
        one = new Solenoid(1);
        two = new Solenoid(2);
    }

    protected void initServos(){
        servo1 = new Servo(6);
        servo2 = new Servo(5);
    }

    /**
     * This method is used to initialize the one Axis 206 KOP camera;
     */
    protected void initCamera(){
        camera = AxisCamera.getInstance();
    }

    /**
     * This method is called each time the robot enters teleop phase.
     * It starts the compressor for the pneumatics and starts the vision thread.
     */
    public void teleopInit(){
        System.out.println("Woot");
        Watchdog.getInstance().setEnabled(true);

        //Start Compressor
        com.start();

        //Start Vision Thread
        //vision = new Vision(camera, 10, null);
        //Thread camThread = new Thread(vision);
        //xcamThread.start();
    }

    public void autonomousInit(){

    }


    //PERIODIC FUNCTIONS //////////////////////////////////////////////////////////
    /**
     * This function is called periodically during autonomous.
     * It feeds a watchdog a bone and moves the robot forward.
     */
    public void autonomousPeriodic() {
        System.out.println("Periodic Auto");
        Watchdog.getInstance().feed();
        driveTrain.drive(1.0, 0);
    }

    /**
     * This method is called periodically during autonomous.
     * It calls the center of found target and prints it.
     */
    public void teleopPeriodic(){
        Watchdog.getInstance().feed();
       //int[] temp = vision.getCenter();
       //System.out.println("X coord : " + temp[0] + "|| Y coord : " + temp[1]);
    }

    public void disabledPeriodic(){
    }

    //CONTINUOUS METHODS //////////////////////////////////////////////////////////

    /**
     * This method is called as fast as possible.
     * It calls three methods: drive, pistons, and servos, to perform teleop functions.
     */
    public void teleopContinuous() {
        Watchdog.getInstance().feed();
        //Much of the below is simply for prototyping

        out.setRaw(255);

        //Sorry Nick if this is wrong, but I wanted to implement some robust system
        try{
            drive();
            pistons();
            servos();
        }catch(NullPointerException e){
            e.printStackTrace();
        }catch(Exception e){
            e.printStackTrace();
        }
    }

    /**
     * This method is used to drive the basic tankdrive.
     * @throws NullPointerException
     */
    protected void drive() throws NullPointerException{
        driveTrain.arcadeDrive(left);
    }

    /**
     * This method is used to manipulate the servos.
     * @throws NullPointerException
     */
    protected void servos()throws NullPointerException{
        //Servos
        if(left.getRawButton(4))
            servo1.set(0.4);
        else
            servo1.set(1.0);
        if(left.getRawButton(5))
            servo2.set(0.6);
        else
            servo1.set(0.0);
    }

    /**
     * This method is used to manipulate the pistons.
     * @throws NullPointerException
     */
    protected void pistons() throws NullPointerException{
        //Pistons
        if(left.getRawButton(1)){
            one.set(true);
            two.set(false);
        }
        else{
            one.set(false);
            two.set(true);
        }
    }

    public void autonomousContinuous(){
    }

    public void disabledContinuous(){
    }
}
